#include "ses_timer.h"
#include "ses_scheduler.h"
#include <stdio.h>

/* A task consists of a task pointer and an expire value. */
typedef struct {
	task_t task;		// pointer to task function
	uint16_t expire;	// expire value (how many updates to wait)
} taskDescriptor_t;



/* Array that holds all scheduled tasks */
volatile taskDescriptor_t tasks[SCHEDULER_ENTRIES];



void scheduler_update(void) {
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		if (tasks[i].expire != 0) {
			--tasks[i].expire;
		}
	}
}

void scheduler_init() {
	sei();

	// initialize scheduler w/ NULL tasks
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		taskDescriptor_t t = { .task = NULL, .expire = 0 };
		tasks[i] = t;
	}

	// start timer2 and begin updating each 1 ms
	timer2_init(scheduler_update);
}

void scheduler_run() {
	task_t task;
	while (1) {
		for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
			if (tasks[i].expire == 0 && tasks[i].task != NULL) {
				cli();
				task = tasks[i].task;
				tasks[i].task = NULL;
				sei();
				task();
			}
		}
	}
}

bool scheduler_add(task_t task, uint16_t expire) {
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		if (tasks[i].task == NULL) {
			tasks[i].task = task;
			tasks[i].expire = expire;
			return true;
		}
	}
	return false;
}

void scheduler_remove(task_t task) {
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		if (tasks[i].task == task) {
			tasks[i].task = NULL;
		}
	}
}
